
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_events.c
  * @author     baiyang
  * @date       2022-3-12
  ******************************************************************************
  */

/*
 *       This event will be called when the failsafe changes
 *       boolean failsafe reflects the current state
 */

/*----------------------------------include-----------------------------------*/
#include "fms.h"

#include <mavproxy/mavproxy.h>
#include <common/time/gp_time.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
bool fms_failsafe_option(enum FailsafeOption opt)
{
    return (fms.g.fs_options & (uint32_t)opt);
}

void fms_failsafe_radio_on_event()
{
    //AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED);

    // set desired action based on FS_THR_ENABLE parameter
    enum FailsafeAction desired_action;
    switch (fms.g.failsafe_throttle) {
        case FS_THR_DISABLED:
            desired_action = FAILSAFE_ACTION_NONE;
            break;
        case FS_THR_ENABLED_ALWAYS_RTL:
        case FS_THR_ENABLED_CONTINUE_MISSION:
            desired_action = FAILSAFE_ACTION_RTL;
            break;
        case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
            desired_action = FAILSAFE_ACTION_SMARTRTL;
            break;
        case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
            desired_action = FAILSAFE_ACTION_SMARTRTL_LAND;
            break;
        case FS_THR_ENABLED_ALWAYS_LAND:
            desired_action = FAILSAFE_ACTION_LAND;
            break;
        case FS_THR_ENABLED_AUTO_RTL_OR_RTL:
            desired_action = FAILSAFE_ACTION_AUTO_DO_LAND_START;
            break;
        default:
            desired_action = FAILSAFE_ACTION_LAND;
    }

    // Conditions to deviate from FS_THR_ENABLE selection and send specific GCS warning
    if (fms_should_disarm_on_failsafe()) {
        // should immediately disarm when we're on the ground
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Radio Failsafe - Disarming");
        arming_disarm(&fms.arming, ARMING_CHECK_RADIOFAILSAFE, true);
        desired_action = FAILSAFE_ACTION_NONE;
#if 0
    } else if (mode_is_landing(fms.flightmode) && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
        // Allow landing to continue when battery failsafe requires it (not a user option)
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Radio + Battery Failsafe - Continuing Landing");
        desired_action = FAILSAFE_ACTION_LAND;
#endif
    } else if (mode_is_landing(fms.flightmode) && fms_failsafe_option(CONTINUE_IF_LANDING)) {
        // Allow landing to continue when FS_OPTIONS is set to continue landing
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Landing");
        desired_action = FAILSAFE_ACTION_LAND;

    } else if (mode_number(fms.flightmode) == AUTO && fms_failsafe_option(RC_CONTINUE_IF_AUTO)) {
        // Allow mission to continue when FS_OPTIONS is set to continue mission
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Auto Mode");       
        desired_action = FAILSAFE_ACTION_NONE;

    } else if ((mode_in_guided_mode(fms.flightmode)) &&
      (fms_failsafe_option(RC_CONTINUE_IF_GUIDED)) && (fms.g.failsafe_gcs != FS_GCS_DISABLED)) {
        // Allow guided mode to continue when FS_OPTIONS is set to continue in guided mode.  Only if the GCS failsafe is enabled.
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Radio Failsafe - Continuing Guided Mode");
        desired_action = FAILSAFE_ACTION_NONE;

    } else {
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Radio Failsafe");
    }

    // Call the failsafe action handler
    fms_do_failsafe_action(desired_action, MODE_REASON_RADIO_FAILSAFE);
}

// failsafe_off_event - respond to radio contact being regained
void fms_failsafe_radio_off_event()
{
    // no need to do anything except log the error as resolved
    // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
    //AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED);
    mavproxy_send_statustext(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared");
}

// failsafe_gcs_check - check for ground station failsafe
void fms_failsafe_gcs_check()
{
    // Bypass GCS failsafe checks if disabled or GCS never connected
    if (fms.g.failsafe_gcs == FS_GCS_DISABLED) {
        return;
    }

    const uint32_t gcs_last_seen_ms = mavproxy_sysid_myggcs_last_seen_time_ms();
    if (gcs_last_seen_ms == 0) {
        return;
    }

    // calc time since last gcs update
    // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs
    const uint32_t last_gcs_update_ms = time_millis() - gcs_last_seen_ms;
    const uint32_t gcs_timeout_ms = (uint32_t)(math_constrain_float(fms.g.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));

    // Determine which event to trigger
    if (last_gcs_update_ms < gcs_timeout_ms && fms.failsafe.gcs) {
        // Recovery from a GCS failsafe
        fms_set_failsafe_gcs(false);
        fms_failsafe_gcs_off_event();

    } else if (last_gcs_update_ms < gcs_timeout_ms && !fms.failsafe.gcs) {
        // No problem, do nothing

    } else if (last_gcs_update_ms > gcs_timeout_ms && fms.failsafe.gcs) {
        // Already in failsafe, do nothing

    } else if (last_gcs_update_ms > gcs_timeout_ms && !fms.failsafe.gcs) {
        // New GCS failsafe event, trigger events
        fms_set_failsafe_gcs(true);
        fms_failsafe_gcs_on_event();
    }
}

// failsafe_gcs_on_event - actions to take when GCS contact is lost
void fms_failsafe_gcs_on_event(void)
{
    //AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED);
    RCs_clear_overrides();

    // convert the desired failsafe response to the FailsafeAction enum
    enum FailsafeAction desired_action;
    switch (fms.g.failsafe_gcs) {
        case FS_GCS_DISABLED:
            desired_action = FAILSAFE_ACTION_NONE;
            break;
        case FS_GCS_ENABLED_ALWAYS_RTL:
        case FS_GCS_ENABLED_CONTINUE_MISSION:
            desired_action = FAILSAFE_ACTION_RTL;
            break;
        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_RTL:
            desired_action = FAILSAFE_ACTION_SMARTRTL;
            break;
        case FS_GCS_ENABLED_ALWAYS_SMARTRTL_OR_LAND:
            desired_action = FAILSAFE_ACTION_SMARTRTL_LAND;
            break;
        case FS_GCS_ENABLED_ALWAYS_LAND:
            desired_action = FAILSAFE_ACTION_LAND;
            break;
        case FS_GCS_ENABLED_AUTO_RTL_OR_RTL:
            desired_action = FAILSAFE_ACTION_AUTO_DO_LAND_START;
            break;
        default: // if an invalid parameter value is set, the fallback is RTL
            desired_action = FAILSAFE_ACTION_RTL;
    }

    // Conditions to deviate from FS_GCS_ENABLE parameter setting
    if (!fms.motors->_armed) {
        desired_action = FAILSAFE_ACTION_NONE;
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS Failsafe");

    } else if (fms_should_disarm_on_failsafe()) {
        // should immediately disarm when we're on the ground
        arming_disarm(&fms.arming, ARMING_CHECK_GCSFAILSAFE, true);
        desired_action = FAILSAFE_ACTION_NONE;
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS Failsafe - Disarming");
#if 0
    } else if (mode_is_landing(fms.flightmode) && ((battery.has_failsafed() && battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY))) {
        // Allow landing to continue when battery failsafe requires it (not a user option)
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS + Battery Failsafe - Continuing Landing");
        desired_action = FAILSAFE_ACTION_LAND;
#endif
    } else if (mode_is_landing(fms.flightmode) && fms_failsafe_option(CONTINUE_IF_LANDING)) {
        // Allow landing to continue when FS_OPTIONS is set to continue landing
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Landing");
        desired_action = FAILSAFE_ACTION_LAND;

    } else if (mode_number(fms.flightmode) == AUTO && fms_failsafe_option(GCS_CONTINUE_IF_AUTO)) {
        // Allow mission to continue when FS_OPTIONS is set to continue mission
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Auto Mode");
        desired_action = FAILSAFE_ACTION_NONE;

    } else if (fms_failsafe_option(GCS_CONTINUE_IF_PILOT_CONTROL) && !mode_is_autopilot(fms.flightmode)) {
        // should continue when in a pilot controlled mode because FS_OPTIONS is set to continue in pilot controlled modes
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS Failsafe - Continuing Pilot Control");
        desired_action = FAILSAFE_ACTION_NONE;
    } else {
        mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS Failsafe");
    }

    // Call the failsafe action handler
    fms_do_failsafe_action(desired_action, MODE_REASON_GCS_FAILSAFE);
}

// failsafe_gcs_off_event - actions to take when GCS contact is restored
void fms_failsafe_gcs_off_event(void)
{
    mavproxy_send_statustext(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared");
    //AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);
}

// set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
//  this is always called from a failsafe so we trigger notification to pilot
void fms_set_mode_RTL_or_land_with_pause(ModeReason reason)
{
    // attempt to switch to RTL, if this fails then switch to Land
    if (!fms_set_mode(RTL, reason)) {
        // set mode to land will trigger mode change notification to pilot
        fms_set_mode_land_with_pause(reason);
    } else {
        // alert pilot to mode change
        notify_events.failsafe_mode_change = 1;
    }
}

bool fms_should_disarm_on_failsafe()
{
    if (fms.ap.in_arming_delay) {
        return true;
    }

    switch (mode_number(fms.flightmode)) {
        case STABILIZE:
        case ACRO:
            // if throttle is zero OR vehicle is landed disarm motors
            return fms.ap.throttle_zero || fms.ap.land_complete;
        case AUTO:
        case AUTO_RTL:
            // if mission has not started AND vehicle is landed, disarm motors
            return !fms.ap.auto_armed && fms.ap.land_complete;
        default:
            // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold
            // if landed disarm
            return fms.ap.land_complete;
    }
}

void fms_do_failsafe_action(enum FailsafeAction action, ModeReason reason)
{
    // Execute the specified desired_action
    switch (action) {
        case FAILSAFE_ACTION_NONE:
            return;
        case FAILSAFE_ACTION_LAND:
            fms_set_mode_land_with_pause(reason);
            break;
        case FAILSAFE_ACTION_RTL:
            fms_set_mode_RTL_or_land_with_pause(reason);
            break;
#if 0
        case FAILSAFE_ACTION_SMARTRTL:
            set_mode_SmartRTL_or_RTL(reason);
            break;
        case FAILSAFE_ACTION_SMARTRTL_LAND:
            set_mode_SmartRTL_or_land_with_pause(reason);
#endif
            break;
        case FAILSAFE_ACTION_TERMINATE: {
            arming_disarm(&fms.arming, ARMING_CHECK_FAILSAFE_ACTION_TERMINATE, true);
            break;
        }
#if 0
        case FAILSAFE_ACTION_AUTO_DO_LAND_START:
            set_mode_auto_do_land_start_or_RTL(reason);
            break;
#endif
        default:
            break;
    }
}

/*------------------------------------test------------------------------------*/


